#include "StdAfx.h"
#include "Pipeline.h"
#include <opencv\cv.h>
#include <opencv\highgui.h>
#include <opencv2\nonfree\features2d.hpp>
#include "Frame.h"
#include <string>

#include "PlayerTracker.h"
#include "LineTracker.h"

#include "ModelMatch.h"

using namespace std;
using namespace cv;

#define tries 10
#define write true

void mouseSelectionCallback(int event, int x, int y, int flags, void* params);

bool drawBox;
Mat selectionFrame;
int boxes = 0;
vector<Rect> trackingWindows;
Rect wBox;

Pipeline::Pipeline(string vidPath, string outPath, CourtModel model, bool trackPlayer) : _vidPath(vidPath), _outPath(outPath), _courtModel(model), _trackPlayer(trackPlayer)
{

}

Pipeline::Pipeline(string vidPath, string outPath, CourtModel model) : _vidPath(vidPath), _outPath(outPath), _courtModel(model)
{
	_trackPlayer = false;
}

//Not USED Preprocesses video to detect hard shifts or camera changes
void Pipeline::preProcess() {

	cout << "PreProcessing Video" << endl;

	_vidStream = VideoCapture(_vidPath);

	int numFrames = (int)_vidStream.get(CV_CAP_PROP_FRAME_COUNT);

	_changes.clear();

	Mat currMat;
	Mat prevMat;
	bool hasFrame = _vidStream.read(prevMat);
	hasFrame = _vidStream.read(currMat);

	for(int i = 1; i < numFrames && hasFrame; i++) {
		bool change = changedViewPoint(currMat, prevMat);
		//add indicator that the view has changed
		if(change) {
			_changes.push_back(i);
		}

		currMat = prevMat;
		hasFrame = _vidStream.read(currMat);
	}
	_vidStream.release();
}

void Pipeline::process() {

	//preProcess();

	_vidStream = VideoCapture(_vidPath);

	double fps = _vidStream.get(CV_CAP_PROP_FPS);

	cvNamedWindow("Pipeline", CV_WINDOW_AUTOSIZE);
	
	static int frameNum=0;

	cout << "Fitting Model" << endl;
	bool hasFrame = incrementFrame();

	//initilized video writer.
	VideoWriter writer;
	if(write){
		Size frameSize(_currFrame._cvFrame.size().width, _currFrame._cvFrame.size().height*2);
		writer.open(_outPath, CV_FOURCC('j', 'p', 'e', 'g'), fps, frameSize, true);
		if(!writer.isOpened()) {
			cout << "Error opening writer!" << endl;
		}

	}

	Mat m = _currFrame._cvFrame.clone();
	_currFrame.drawFrame(m, true);
	
	//construct the frame to write.
	if(write){
		Mat outputFrame (_currFrame._cvFrame.rows*2, _currFrame._cvFrame.cols, _currFrame._cvFrame.type());
		Rect roi (0, 0, _currFrame._cvFrame.cols , _currFrame._cvFrame.rows );
		Mat orig (outputFrame, roi);
		_currFrame._cvFrame.copyTo(orig);
		Rect newRoi (0, 720, _currFrame._cvFrame.cols , _currFrame._cvFrame.rows );
		Mat line (outputFrame, newRoi);
		m.copyTo(line);
		writer << outputFrame;
	}
	/*imshow("Pipeline", m);
	cvWaitKey(0);*/

	//initilizae the model matcher
	ModelMatch matcher(_currFrame, _courtModel);
	//vector<Line> lines = _currFrame.findBoundingLines();
	vector<Intersection> rwIntSecs = _currFrame.getIntSecs();
	vector<Intersection> mIntSecs = _courtModel._intersections;
	vector<Line> mHLines = _courtModel.getHorLines();
	vector<Line> mVLines = _courtModel.getVerLines();
	
	//build image of courtmodel to be used for visual output.
	Mat modelMat(_currFrame._cvFrame.size(), _currFrame._cvFrame.type());
	int xOffset = (_currFrame._cvFrame.cols - _courtModel.cols*1/2)/2;
	int yOffset = (_currFrame._cvFrame.rows - _courtModel.rows*1/2)/2;
	for(int i=0; i<mHLines.size(); i++){
		Point p1 = mHLines[i].getPoints()[0];
		Point p2 = mHLines[i].getPoints()[1];

		Point newP1 = Point(p1.x*1/2 + xOffset, p1.y*1/2+yOffset);
		Point newP2 = Point(p2.x*1/2 + xOffset, p2.y*1/2+yOffset);
		Line l(newP1, newP2);
		l.drawLine(modelMat, Scalar(255,255, 255));
	}
	for(int i=0; i<mVLines.size(); i++){
		Point p1 = mVLines[i].getPoints()[0];
		Point p2 = mVLines[i].getPoints()[1];

		Point newP1 = Point(p1.x*1/2 + xOffset, p1.y*1/2+yOffset);
		Point newP2 = Point(p2.x*1/2 + xOffset, p2.y*1/2+yOffset);
		Line l(newP1, newP2);
		l.drawLine(modelMat, Scalar(255,255, 255));
	}

	Mat H;
	//matcher.fitModelAbsolute(H, lines[0], lines[1], lines[2], lines[3]);
	double score = matcher.fastModelExaustiveFit(H);
	//matcher.pointModelExaustiveFit(H);
	oldScore = score;

	selectionFrame = _currFrame._cvFrame.clone();
	cout << "Int secs: " << mIntSecs.size() << endl;
	for(size_t i = 0; i < mIntSecs.size(); i++) {
		Intersection intsec = mIntSecs[i];
		Point pt = intsec.intSecPt;

		Point newPt = matcher.translatePPrimetoP(pt, H);
		circle(selectionFrame, newPt, 5, Scalar(255,255,0), 5);
	}

	if(write){
		Mat outputFrame (_currFrame._cvFrame.rows*2, _currFrame._cvFrame.cols, _currFrame._cvFrame.type());
		Rect roi (0, 0, _currFrame._cvFrame.cols , _currFrame._cvFrame.rows );
		Mat orig (outputFrame, roi);
		_currFrame._cvFrame.copyTo(orig);
		//Point p (currMat.rows+1, 0);
		Rect newRoi (0, 720, _currFrame._cvFrame.cols , _currFrame._cvFrame.rows );
		Mat line (outputFrame, newRoi);
		m.copyTo(line);
		writer << outputFrame;
	}
	imshow("Pipeline", selectionFrame);

	//Prompts user to draw rectangles around player's position in first frame if user wants to track the players.
	PlayerTracker pTracker;
	if(_trackPlayer){
		Mat start;
		selectionFrame.copyTo(start);
		drawBox = false;
		cvSetMouseCallback("Pipeline", mouseSelectionCallback);
		while(boxes < 2) {
				imshow("Pipeline", selectionFrame);
				cvWaitKey(33);
		}
		Rect p1Win = trackingWindows[0];
		Rect p2Win = trackingWindows[1];
		pTracker = PlayerTracker(start, p1Win, p2Win);
	}

	vector<Line> horLines = _currFrame.getHorLines();
	vector<Line> verLines = _currFrame.getVerLines();

	while(hasFrame) {
		//vector<Line> lines = _currFrame.findBoundingLines();

		Mat H;
		double score ;
		matcher.updateFrame(_currFrame);
		//matcher.fitModelAbsolute(H, lines[0], lines[1], lines[2], lines[3]);
		score = matcher.fastModelExaustiveFit(H);
		//double score = matcher.pointModelExaustiveFit(H);
		//matcher.printMatrix(H);

		oldScore = score;

		Mat currMat = _currFrame._cvFrame.clone();
		Mat prevMat = _prevFrame._cvFrame;
		
		if(_trackPlayer){
			//updating player tracker frame;
			pTracker.updateFrame(currMat);
		}


		if(H.type() == 0){
			cout<< "pipeline" << endl; 
		}

		//drawing projected intersections
		for(size_t i = 0; i < mIntSecs.size(); i++) {
			Intersection intsec = mIntSecs[i];
			Point pt = intsec.intSecPt;

			Point newPt = matcher.translatePPrimetoP(pt, H);
			circle(currMat, newPt, 5, Scalar(255,255,0), 5);
		}

		//drawing projected lines
		for(int i=0; i<mHLines.size(); i++){
			Point p1 = mHLines[i].getPoints()[0];
			Point p2 = mHLines[i].getPoints()[1];

			Point newP1 = matcher.translatePPrimetoP(p1, H);
			Point newP2 = matcher.translatePPrimetoP(p2, H);

			Line l(newP1,newP2);
			l.drawLine(currMat, Scalar(128,255,128));
		}
		for(int i=0; i<mVLines.size(); i++){
			Point p1 = mVLines[i].getPoints()[0];
			Point p2 = mVLines[i].getPoints()[1];

			Point newP1 = matcher.translatePPrimetoP(p1, H);
			Point newP2 = matcher.translatePPrimetoP(p2, H);

			Line l(newP1,newP2);
			l.drawLine(currMat, Scalar(128,255,128));
		}

		//processing output.
		horLines = _currFrame.getHorLines();
		verLines = _currFrame.getVerLines();
		if(write){
			char numstr[21]; // enough to hold all 64-bits numbers 
			sprintf(numstr, "%f", score);
			string text = "";
			string result =  text + numstr;
			putText(currMat, result , cvPoint(30,680), 
				FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,250), 1, CV_AA);
			Mat m;
			if(!_trackPlayer){
				 m = _currFrame._cvFrame.clone();
				_currFrame.drawFrame(m, true);
			}
			//output player location on model
			else{
				m = modelMat.clone();
				Point2f p1Centroid;
				pTracker.getPlayer(1).centroid(p1Centroid);
				Point2f p2Centroid;
				pTracker.getPlayer(2).centroid(p2Centroid);
			
				Point p1Point (p1Centroid.x, p1Centroid.y);
				Point p2Point (p2Centroid.x, p2Centroid.y);

				Point newP1 = matcher.translatePtoPPrime(p1Point, H);
				Point newP2 = matcher.translatePtoPPrime(p2Point, H);

				Point player1 (newP1.x*1/2 + xOffset, newP1.y*1/2+yOffset);
				Point player2 (newP2.x*1/2 + xOffset, newP2.y*1/2+yOffset);

				putText(m, "P1" , player1, 
					FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,250), 1, CV_AA);
				putText(m, "P2" , player2, 
					FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,250,0), 1, CV_AA);
			}
			
			Mat outputFrame (currMat.rows*2, currMat.cols, currMat.type());
			Rect roi (0, 0, currMat.cols , currMat.rows );
			Mat orig (outputFrame, roi);
			currMat.copyTo(orig);
			//Point p (currMat.rows+1, 0);
			Rect newRoi (0, 720, currMat.cols , currMat.rows );
			Mat line (outputFrame, newRoi);
			m.copyTo(line);
			writer << outputFrame;

			//imwrite("sample.png", outputFrame);
		}
		imshow("Pipeline", currMat);
		cvWaitKey(30);
		hasFrame = incrementFrame();
		frameNum++;
	}
	if(write){
		writer.release();
	}
}


//NOT USED. Detecting shot change
bool Pipeline::changedViewPoint(Frame curr, Frame prev) {

	Mat currMat = curr._cvFrame;
	Mat prevMat = prev._cvFrame;

	cvtColor(currMat, currMat, CV_BGR2GRAY);
	cvtColor(prevMat, prevMat, CV_BGR2GRAY);

	if(!currMat.data && !prevMat.data ) {
		return false;
	}

	Mat diff = currMat - prevMat;

	int changeSum = 0;
	for(int r = 0; r < diff.rows; r++) {
		for(int c = 0; c < diff.cols; c++) {
			uchar currVal = currMat.at<uchar>(r,c);
			uchar prevVal = prevMat.at<uchar>(r,c);
			if(abs(currVal - prevVal) > 25) {
				changeSum++;
			}
		}
	}

	double ratio = (double)changeSum / (currMat.cols * currMat.rows);

	return (ratio > 50);
}

// if the 2 frames SSD is over 50 percent than return true.
bool Pipeline::changedViewPoint(Mat curr, Mat prev) {

	Mat currMat;
	Mat prevMat;

	curr.copyTo(currMat);
	prev.copyTo(prevMat);

	cvtColor(currMat, currMat, CV_BGR2GRAY);
	cvtColor(prevMat, prevMat, CV_BGR2GRAY);

	if(!currMat.data && !prevMat.data ) {
		return false;
	}

	Mat diff = currMat - prevMat;

	int changeSum = 0;
	for(int r = 0; r < diff.rows; r++) {
		for(int c = 0; c < diff.cols; c++) {

			uchar diffVal = diff.at<uchar>(r,c);

			if(abs(diffVal) > 25) {
				changeSum++;
			}
		}
	} 

	double ratio = (double)changeSum / (diff.cols * diff.rows);

	return (ratio > 50);
}

Frame Pipeline::getFrame() 
{
	return _currFrame;
}

//Grabbing the next frame in video as current and keep the current frame as prev
bool Pipeline::incrementFrame()
{
	static int frameNum=0;
	Mat m; 
	
	_prevFrameMat = _currFrame._cvFrame.clone();

	bool hasFrame = _vidStream.read(m);
	if(hasFrame)
	{
		frameNum++;
		_prevFrame = _currFrame;
		_currFrame = Frame(m);
	}
	return hasFrame;
}

Pipeline::~Pipeline(void)
{
}

//handling box drawing around players
void mouseSelectionCallback(int event, int x, int y, int flags, void* params) {
	switch(event) {
	case CV_EVENT_MOUSEMOVE:
		if(drawBox) {
			wBox.width = x - wBox.x;
			wBox.height = y - wBox.y;
		}
		break;
	case CV_EVENT_LBUTTONDOWN:
		drawBox = true;
		wBox = Rect(x,y,0,0);
		break;
	case CV_EVENT_LBUTTONUP:
		drawBox = false;
		boxes++;
		if(wBox.width < 0) {
			wBox.x += wBox.width;
			wBox.width *= -1;
		}

		if(wBox.height < 0) {
			wBox.y += wBox.height;
			wBox.height *= -1;
		}

		rectangle(selectionFrame, cvPoint(wBox.x,wBox.y), 
			cvPoint(wBox.x + wBox.width, wBox.y + wBox.height),
			cvScalar(0xff, 0, 0));
		trackingWindows.push_back(wBox);
		break;
	}
}
